% clear
% clc
target_point=[5,5];
car_point=[0,0];
tio_cos=dot([1,0],target_point)/norm(target_point);
tio=acos(tio_cos);
if(target_point(2)<0)
    tio=-tio;
end

time_step=0.05;
v=0;
w=0;
car_angle=0;
vg=[];
vLg=[];
vRg=[];
wg=[];
sigg=[];
for i=1:200
    P=[car_point(1);car_point(2);car_angle];
    q=[v;w];
    P=P+time_step*[cos(car_angle),0;sin(car_angle),0;0,1]*q;
    car_point=[P(1),P(2)];
    car_angle=P(3);
    plot(car_point(1),car_point(2),'r*');
    hold on
    car_dir=[1*cos(car_angle),1*sin(car_angle)];
    dg=norm(target_point-car_point);
    sig_dot=dot(car_dir,target_point-car_point);
    sig_cos=sig_dot/(dg);
    sig=acos(sig_cos);
    if(car_angle>=tio)
        sig=-sig;
    end
    if(sig>1.4)
        sig=1.4;
    elseif(sig<-1.4)
        sig=-1.4;
    end
    sigg=[sigg,sig];
    do=2;
    sio=-pi/2;
    v_o=v+rand(1);
    w_o=w+rand(1);
    if(dg<0.3)
        [vR,vL]=[0,0];
    else
        [vR,vL]=RFNN_EKF_cal_v(dg,do,sig,sio,v_o,w_o);
        %[vL,vR]=evalfis(rfnnModel_v5_linear,[dg,do,sig,sio]);
    end
    v=(vL+vR)/2;
    w=(vR-vL)/(2*0.098);
    vg=[vg,v];
    vLg=[vLg,vL];
    vRg=[vRg,vR];
    wg=[wg,w];
end
% dg=5;
% do=0.5;
% sig=-pi/3;
% sio=pi/6;
% v_o=0;
% w_o=0;
% [vR,vL,Fnk]=RFNN_EKF_cal_v(dg,do,sig,sio,v_o,w_o);